#define __LIBSCOS
#define LIBMAIN
#define LIBCTOR scos_init
#define LIBDTOR scos_fini

#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include "scos.h"
#include "scutable.h"

#define SCOS_TRACE_MAX 65536
#define LCM_FILENAME            ("/flash/RapIB311XetalLcm")

scos_mutex_t socketmutex;
table_t sockets;

size_t totalb=0, totala=0;
static int mutexes=0, conditions=0;
static int tasks=0;
unsigned char scos_tracebuffer[SCOS_TRACE_MAX];
int scos_tracepointer=0;

/* C++ initialization */
void _main(void);

void MAIN(void);

void Main()
{
	RapOpen(LCM_FILENAME);
    RapInitialise();

    t_setpri(0, SCOS_PRIO, NULL);

	printf("[SCPSOS] Starting SmartCam OS v0.01\n");

    //_main();

	scos_mutex_init(&socketmutex);
	tblcreate(&sockets, sizeof(scos_socket_t));

	printf("[SCPSOS] Running application\n\n");

	sctmmain();

	printf("[SCPSOS] Got back from main function\n");
}

LIBCTOR void scos_init(void)
{
}

LIBDTOR void scos_fini(void)
{
}

LIBSYMBOL void scos_dump(void)
{
	struct rninfo rninfo;
	struct tinfo tinfo;
	char buffer[16];
	unsigned long actlen;
  
    memset(&rninfo, 0, sizeof(rninfo));
    memset(&tinfo, 0, sizeof(tinfo));

  	sys_info(PSOS_VERSION, buffer, PATH_MAX, &actlen);
	printf("\n[SCPSOS] Running on PSOS version %s\n", buffer);
	printf("[SCPSOS] {NTASK, NMUTEX, NCVAR, NQUEUE} = {%d, %d, %d, %d}\n",
		   KC_NTASK, KC_NMUTEX, KC_NCVAR, KC_NQUEUE);
	t_info(0, &tinfo);
	printf("[SCPSOS] Task (\"%c%c%c%c\"): Priority %d, %ld bytes stack. %ld ticks per timeslice\n",
		   tinfo.name[0], tinfo.name[1], tinfo.name[2], tinfo.name[3], 
		   tinfo.cpriority, tinfo.ss_size, tinfo.tslice_quantum);

	rn_info(0, &rninfo);
	printf("[SCPSOS] Region 0 (\"%c%c%c%c\"): %ld bytes starting at %p.\n[SCPSOS] %ld bytes free, largest chunk %ld bytes\n\n",
		   rninfo.name[0],rninfo.name[1],rninfo.name[2],rninfo.name[3],
		   rninfo.length, rninfo.start_addr, rninfo.free_bytes, rninfo.largest);
}

static void scos_thread_create_helper(void *(*func)(void*), void *arg)
{
    printf("[SCPSOS] New task\n");
    
    func(arg);

    printf("[SCPSOS] Task %d exited\n", --tasks);

    t_delete(0);
}

LIBSYMBOL int  scos_thread_create(scos_thread_t *thread, void* (*func)(void*), void *arg)
{
	unsigned long args[4];

    args[0] = (unsigned long)func;
	args[1] = (unsigned long)arg;

    printf("[SCPSOS] Starting task %d with entry point %p, argument vector %p\n", tasks, func, arg);

	t_create("name", SCOS_PRIO, 100000, 100000, T_LOCAL|T_FPU, thread);
	t_start(*thread, T_PREEMPT|T_NOTSLICE|T_NOASR|T_USER|T_ISR, scos_thread_create_helper, args);

    printf("[SCPSOS] Task %d started\n", tasks++);

	return 0;
}

LIBSYMBOL int  scos_thread_join(scos_thread_t *thread)
{
	/* Never exit */
	while (1) usleep(1000000L);
	return 0;
}

LIBSYMBOL int  scos_mutex_init(scos_mutex_t *mutex)
{
    static int nr = 0;
    int error = mu_create("name", MU_LOCAL|MU_RECURSIVE|MU_PRIO_NONE, 0, mutex);
    //printf("initializing mutex %d\n", mutexes++);
    if (error) { printf("scos_mutex_init: error %d\n", error); exit(-1); }
	return 0;
}

LIBSYMBOL int  scos_mutex_destroy(scos_mutex_t *mutex)
{
	int error = mu_delete(*mutex);
    //printf("destroying mutex %d\n", --mutexes);
    if (error) printf("scos_mutex_destroy: error %d\n", error);

	return 0;
}

LIBSYMBOL int  scos_mutex_lock(scos_mutex_t *mutex)
{
	int error = mu_lock(*mutex, MU_WAIT, 0);
    if (error) printf("scos_mutex_lock: error %d\n", error);

	return 0;
}

LIBSYMBOL int  scos_mutex_unlock(scos_mutex_t *mutex)
{
	int error = mu_unlock(*mutex);
    if (error) printf("scos_mutex_unlock: error %d\n", error);

	return 0;
}

LIBSYMBOL int  scos_mutex_trylock(scos_mutex_t *mutex)
{
	return mu_lock(*mutex, MU_NOWAIT, 0);
}

LIBSYMBOL int  scos_cond_init(scos_cond_t *cond)
{
	int error = cv_create("name", CV_LOCAL|CV_FIFO, cond);
    //printf("initializing condition %d\n", conditions++);
    if (error) { printf("scos_cond_init: error %d\n", error); exit(-1); }
	return 0;
}

LIBSYMBOL int  scos_cond_wait(scos_cond_t *cond, scos_mutex_t *mutex)
{
	int error = cv_wait(*cond, *mutex, 0);
    if (error) printf("scos_cond_wait: error %d\n", error);

	return 0;
}

LIBSYMBOL int  scos_cond_signal(scos_cond_t *cond)
{
	int error = cv_broadcast(*cond);
    if (error) printf("scos_cond_signal: error %d\n", error);

	return 0;
}

LIBSYMBOL int  scos_cond_destroy(scos_cond_t *cond)
{
	int error = cv_delete(*cond);
    //printf("destroying condition %d\n", --conditions);
    if (error) printf("scos_cond_destroy: error %d\n", error);
	return 0;
}

LIBSYMBOL int scos_self(void)
{
	unsigned long tid;

	t_ident(NULL, 0, &tid);

	return tid;
}

#undef malloc
LIBSYMBOL void*scos_malloc(size_t size)
{
  void *data;
  
  printf("[SCPSOS] malloc start\n");

  data = malloc(size);

  totala++;
  totalb += size;

  printf("[SCPSOS] %p = malloc(%ld); %ld bytes in %ld allocations\n", data, size, totalb, totala);

  return data;
}

#undef calloc
LIBSYMBOL void*scos_calloc(size_t nmemb, size_t size)
{
  void *data;

  printf("[SCPSOS] malloc start\n");

  data = calloc(nmemb, size);

  totala++;
  totalb += nmemb*size;

  printf("[SCPSOS] %p = calloc(%ld, %ld); %ld bytes in %ld allocations\n", data, nmemb, size, totalb, totala);

  return data;
}

LIBSYMBOL void usleep(unsigned long int usec)
{
	tm_wkafter(usec/1000);
}

LIBSYMBOL int gettimeofday(struct timeval *tv, void *dummy)
{
	unsigned long tickshi, tickslo;
	double temp;

	tm_getticks(&tickshi, &tickslo);
	temp = (double)tickshi * (1<<31) * 2.0 + tickslo;

	tv->tv_sec = temp/1000;
	tv->tv_usec = (tickslo%1000)*1000;

	return 0;
}

LIBSYMBOL void sched_yield(void)
{
	tm_wkafter(0);
}

LIBSYMBOL struct hostent *gethostbyname(char *name)
{
	return NULL;
}

#undef LOCK
#undef UNLOCK
#undef UNLOCKRETI
#undef UNLOCKRETP

#define LOCK()             scos_mutex_lock(&socketmutex)
#define UNLOCK()           scos_mutex_unlock(&socketmutex)
#define UNLOCKRETI(retval) { int _retval = retval; UNLOCK(); return _retval; }
#define UNLOCKRETP(retval) { void *_retval = retval; UNLOCK(); return _retval; }

int scos_socket (int af, int type, int protocol)
{
	LOCK();

#if 0
	/* Local, streaming sockets should be enough for everyone */
	if (af != AF_UNIX) UNLOCKRETI(-1);
	if (type != SOCK_STREAM) UNLOCKRETI(-1);
#endif

	UNLOCKRETI(tbladdnew(&sockets, NULL));
}

int scos_bind   (int s, struct sockaddr *addr, int   addrlen )
{
	scos_socket_t *socket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (socket->bound) UNLOCKRETI(-1);
	
	memcpy(&socket->local_addr, addr, addrlen);
	socket->local_addrlen = addrlen;

	socket->bound = 1;

	UNLOCKRETI(0);
}

/* Whatever the value of backlog, we use 1 */
int scos_listen (int s, int backlog)
{
	scos_socket_t *socket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (!socket->bound) UNLOCKRETI(-1);
	if (socket->listening) UNLOCKRETI(-1);

	socket->listening = 1;

	UNLOCKRETI(0);
}

int scos_accept (int s, struct sockaddr *addr, int  *addrlen)
{
	scos_socket_t *socket, *newsocket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (!socket->listening) UNLOCKRETI(-1);

	while (!socket->remote_addrlen)
		scos_cond_wait(&socket->condition, &socketmutex);

	tbladdnew(&sockets, &newsocket);

	memcpy(newsocket, socket, sizeof(scos_socket_t));
	*addrlen = newsocket->remote_addrlen;
	memcpy(addr, &newsocket->remote_addr, *addrlen);
	
	socket->remote_addrlen = 0;
	newsocket->listening = 0;
	newsocket->connected = 1;

	UNLOCKRETI(0);
}

int scos_connect(int s, struct sockaddr *addr, int   addrlen )
{
	ULONG err;
	scos_socket_t *socket, *remote;
	int id=-1;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (!socket->bound) UNLOCKRETI(-1);
	if (socket->listening) UNLOCKRETI(-1);
	if (socket->connected) UNLOCKRETI(-1);

	while (tblget(&sockets, id = tblgetnexti(&sockets, id), &remote))
	{
		/* Backlog of 1 */
		if (remote->local_addrlen == addrlen &&
			!memcmp(&remote->local_addr, addr, addrlen) &&
			remote->listening && !remote->remote_addrlen)
			break;
	}

	if (remote)
	{
		if (err = q_vcreate("queu", Q_LOCAL|Q_FIFO, 1, SCOS_MTU, &socket->qid))
		{
			printf("q_vcreate: %ld\n", err);
			errno = ENODEV;
			UNLOCKRETI(-1);
		}

		memcpy(&remote->remote_addr, &socket->local_addr, socket->local_addrlen);
		remote->remote_addrlen = socket->local_addrlen;
		remote->qid = socket->qid;
		socket->connected = 1;
			
		scos_cond_signal(&remote->condition);
	}
	else
	{
		errno = ECONNREFUSED;
		UNLOCKRETI(-1);
	}

	UNLOCKRETI(0);
}

int scos_recv   (int s, char  * buf, int len, int flags)
{
	ULONG msg_len;
	scos_socket_t *socket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (!socket->connected) UNLOCKRETI(-1);
	if (socket->listening) UNLOCKRETI(-1);

	UNLOCK();

	if (q_vreceive(socket->qid, Q_WAIT, 0, buf, len, &msg_len))
		return -1;

	return msg_len;
}

int scos_send   (int s, char  * buf, int len, int flags)
{
	scos_socket_t *socket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);
	if (!socket->connected) UNLOCKRETI(-1);
	if (socket->listening) UNLOCKRETI(-1);

	UNLOCK();

	if (q_vsend(socket->qid, buf, len))
		return -1;

	return len;
}

int scos_close  (int s)
{
	scos_socket_t *socket;

	LOCK();

	if (!tblget(&sockets, s, &socket)) UNLOCKRETI(-1);

	tbldel(&sockets, s, 1);

	UNLOCKRETI(0);
}

int scos_sendto (int s, char *msg, int len, int flags,
                 struct sockaddr *to, int tolen)
{
	/* Connectionless mode unsupported */
	return -1;
}

int             scos_setsockopt(int s, int level, int optname, const void*optval,
                                int optlen)
{
  return 0;
}

void scos_memtest(void)
{
  int i;

  printf(" *** starting memory test ***\n");

  for (i=0; i < 2000; i++)
    scos_malloc(rand()%1024);

  printf(" *** memory test completed ***\n");
}

void scos_trace(unsigned char op)
{
  FILE *f;
  static int done=0;

  return;

  //scos_mutex_lock(&socketmutex);
  if (!done)
  {
    scos_tracebuffer[scos_tracepointer++] = op;
    if (scos_tracepointer == SCOS_TRACE_MAX)
    {
      /* Write trace */
      done = 1;
      f = fopen("/flash/tracebuffer", "w");
      fwrite(scos_tracebuffer, sizeof(unsigned char), SCOS_TRACE_MAX, f);
      fclose(f);
      exit(-1);
    }
  }
  //scos_mutex_unlock(&socketmutex);
}

LIBSYMBOL void scos_setpri(unsigned char prio)
{
  //printf("setting thread %d to priority %d\n", scos_self(), prio);
  t_setpri(0, prio, NULL);
  //printf("thread %d set to priority %d\n", scos_self(), prio);
}

LIBSYMBOL void scos_setpreempt(unsigned char yesno)
{
/*
  if (yesno)
    t_mode(T_PREEMPT|T_NOPREEMPT, T_PREEMPT, NULL);
  else
    t_mode(T_PREEMPT|T_NOPREEMPT, T_NOPREEMPT, NULL);
*/
  //printf("task %d now %s preemptible\n", scos_self(), yesno?"":"NOT");
}
